#include "WiiCIS.h"
#include <iostream>
#include <ctime>

using namespace WiiCIS;
using namespace std;

void wait(double seconds) {
	double endwait = std::clock() + seconds * CLK_TCK;
	while (std::clock() < endwait) {}
}

int main() {
	cout << "Welcome to WiiCIS Demo v2. Press 1 & 2 to connect." << endl;

	Wiimote wiimote = Wiimote();
	while (!wiimote.Connect()) { }

	wiimote.setRumble(true);
	wait(0.25);
	wiimote.setRumble(false);

	wiimote.setLeds(WMLED_1);

	WiimoteState state = wiimote.getState();

	while (wiimote.isConnected()) {
		WiimoteState newState = wiimote.getState();

		if (memcmp(&state, &newState, sizeof(WiimoteState)) == 0)
			continue;

		memcpy(&state, &newState, sizeof(WiimoteState));

		if (state.isReleased((Button)65535))
			continue;

		if (state.isPressed(WMBTN_HOME))
			break;
		if (state.isPressed(WMBTN_A))
			cout << "A Pressed" << endl;
		if (state.isPressed(WMBTN_B)) {
			cout << "B Pressed" << endl;
			wiimote.setRumble(true);
			wait(0.25);
			wiimote.setRumble(false);
		}
		if (state.isPressed(WMBTN_LEFT))
			cout << "Left Pressed" << endl;
		if (state.isPressed(WMBTN_RIGHT))
			cout << "Right Pressed" << endl;
		if (state.isPressed(WMBTN_UP))
			cout << "Up Pressed" << endl;
		if (state.isPressed(WMBTN_DOWN))
			cout << "Down Pressed" << endl;
		if (state.isPressed(WMBTN_PLUS))
			cout << "+ Pressed" << endl;
		if (state.isPressed(WMBTN_MINUS))
			cout << "- Pressed" << endl;
		if (state.isPressed(WMBTN_ONE))
			cout << "1 Pressed" << endl;
		if (state.isPressed(WMBTN_TWO))
			cout << "2 Pressed" << endl;
	}

	if (!wiimote.isConnected())
		cout << "Disconnected!?";

	wiimote.setLeds(WMLED_NONE);

	wiimote.Disconnect();
}